function [u,I,energy, energy0] = ClusterTV(xtol,opt_energy)
%%
%   Function ClusterTV
%
%   Decription: u0 is the input 2-D data
%		lambda is the penalty parameter
%		n is the number of labels
%       u is the output image
%       I is the labeling function

u0 = double(imread('E:\Fun\SeniorThesis\images\gray\noise0.01.bmp'))/255; %input image
%u0 = double(imread('exp2.png'))/255; %input image

%%constant values exp2
% c(1) = 0;
% c(2) = 85/255;  
% c(3) = 170/255;  
% c(4) = 1;
% 


%%constant values brain
c(1) = 0.1744;
c(2) = 0.4086;%sum(sum(mask(:,:,2).*u0))/(sum(sum(mask(:,:,2)))); %85/255;
c(3) = 0.6160;%sum(sum(mask(:,:,3).*u0))/(sum(sum(mask(:,:,3)))); %170/255;
%c(4) = sum(sum(mask(:,:,4).*u0))/(sum(sum(mask(:,:,4))));%+0.055; %255/255;
%------------
c(4) = 0.8250;

delta = 0.005; %time step size
s = 0.01;


%regularization parameter
lambda = 0.08;  %brain.png
%lambda = 0.2;  %exp2.png
n = 4;


energy=zeros(500,1);

nx = size(u0,2); % number of columns
ny = size(u0,1); % number of rows

f = zeros(ny,nx,n);


%data term
for i = 1:n
    f(:,:,i) = sqrt((u0 - c(i)).^2);
end



Divp = zeros(ny,nx,n);
w = zeros(ny,nx,n);

p1 = zeros(ny+1,nx,n); % y direction
p2 = zeros(ny,nx+1,n); % x direction

D1_rho = zeros(ny+1,nx,n);
D2_rho = zeros(ny,nx+1,n);

l = zeros(ny,nx);
 

norm = 100000;

teller = 0;
stop=0;
while(~stop)  %stopping criteria brain.png
%while(norm > 5e-4)   %stopping criteria exp2.png
    
    teller = teller + 1;
          

sum_w = zeros(ny,nx);

for(i=1:n)
    Divp(:,:,i) = p1(2:ny+1,:,i) - p1(1:ny,:,i) + p2(:,2:nx+1,i) - p2(:,1:nx,i);
    w(:,:,i) = exp( (-f(:,:,i) - Divp(:,:,i))/s );
  %  ww(:,:,i) = f(:,:,i) + Divp(:,:,i);
    sum_w = sum_w + w(:,:,i);
end



%fprintf('iter=%d, energy=%f\n',teller, energy);
% gradient-descent step
for(i=1:n)
    %rho(:,:,i) = -w(:,:,i) ./ (sum_w + 0.0001);
    rho(:,:,i) = -w(:,:,i) ./ (sum_w + 1e-20);
    D1_rho(2:ny,:,i) = rho(2:ny,:,i) - rho(1:ny-1,:,i);
    D2_rho(:,2:nx,i) = rho(:,2:nx,i) - rho(:,1:nx-1,i);
    
    
    D1_rho(1,:,i) = 0; 
    D1_rho(ny+1,:,i) = 0; 
    
    D2_rho(:,1,i) = 0; 
    D2_rho(:,nx+1,i) = 0; 
    
    P1(:,:,i) = p1(:,:,i) + delta*D1_rho(:,:,i);
    P2(:,:,i) = p2(:,:,i) + delta*D2_rho(:,:,i);
    
end

% projection step
for(i=1:n)
    l = sqrt( 0.5*( P1(2:ny+1,:,i).^2 + P1(1:ny,:,i).^2 +...
	      P2(:,2:nx+1,i).^2 + P2(:,1:nx,i).^2 ));
      

    
   %l = double(l <= lambda) + lambda*double(~(l <= lambda))./(l+ 1e-20);
   l = double(l <= lambda) + double(~(l <= lambda)).*(l./lambda);
  
   l(:,:) = 1 ./ l ;
   
   q1 = [zeros(1,nx); 0.5*(l(2:ny,:) + l(1:ny-1,:)); zeros(1,nx)];
   q2 = [zeros(ny,1), 0.5*(l(:,2:nx) + l(:,1:nx-1)), zeros(ny,1)];
   
   p1last(:,:,i) = p1(:,:,i);
   p2last(:,:,i) = p2(:,:,i);
   
   p1(:,:,i) = P1(:,:,i).*q1(:,:);
   p2(:,:,i) = P2(:,:,i).*q2(:,:);
    
end
energy(teller)=Smootheddual_energy(p1,p2,f,s);
if (opt_energy~=0)
stop=abs((energy(teller)-opt_energy)./opt_energy) <xtol||teller>500;
else
    norm = sum(sum(sum( sqrt( (p1).^2 )))) + sum(sum(sum( sqrt((p2).^2) )));
    norm = (sum(sum(sum( sqrt( (p1-p1last).^2 )))) + sum(sum(sum( sqrt((p2-p2last).^2) ))))/norm;
   stop=(norm <xtol||teller>500);
end
%     Divp = p1(2:ny+1,:,:) - p1(1:ny,:,:) + p2(:,2:nx+1,:) - p2(:,1:nx,:);
% [uu,I] = min(Divp + f, [], 3);
%     
%     u = zeros(ny,nx);
% for(i=1:n)
%     u(:,:) = u(:,:) + c(i)*double(I==i);
% end
% 
% 
% if(mod(teller,10) == 0 & teller > 100)
% %   file(1:7) = 'output1';
% %   file(8) = '0' + char((teller-100)/10);
% %   file(9:12) = '.png';
% 
% imwrite(uint8(255*u),'output.png');
% end

end

energy(teller+1:end)=[];

Divp = p1(2:ny+1,:,:) - p1(1:ny,:,:) + p2(:,2:nx+1,:) - p2(:,1:nx,:);

[uu,I] = min(Divp + f, [], 3);
energy0=sum(sum(uu));

u = zeros(ny,nx);
for(i=1:n)
    u(:,:) = u(:,:) + c(i)*double(I==i);
end


imwrite(uint8(255*u),'output.png');

